// Copyright (c) 2024, lfr
// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <limits>
#include <vector>

#include "my_hardware_interface/wheel_legged_robot_hardware_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace my_hardware_interface
{
CallbackReturn WheelLeggedRobotHardwareInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
  {
    return CallbackReturn::ERROR;
  }

  // TODO(anyone): read parameters and initialize the hardware
  hw_position_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_velocity_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_effort_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_effort_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN()); 

  return CallbackReturn::SUCCESS;
}

CallbackReturn WheelLeggedRobotHardwareInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
  // 初始化各个硬件的代码
  left_socketcan_.socketcan_init("can0");
  right_socketcan_.socketcan_init("can1");
  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> WheelLeggedRobotHardwareInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_states_[i]));
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_states_[i]));
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_effort_states_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> WheelLeggedRobotHardwareInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      // TODO(anyone): insert correct interfaces
      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_effort_commands_[i]));
  }

  return command_interfaces;
}

CallbackReturn WheelLeggedRobotHardwareInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to receive commands
  left_socketcan_.wheel_motor_start();
  left_socketcan_.leg_motor_start();
  right_socketcan_.wheel_motor_start();
  right_socketcan_.leg_motor_start();

  // 读取当前轮毂电机位置作为位置偏置
  left_wheel_position_offset_ = left_socketcan_.wheel_motor_angle_read();
  right_wheel_position_offset_ = right_socketcan_.wheel_motor_angle_read();
  return CallbackReturn::SUCCESS;
}

CallbackReturn WheelLeggedRobotHardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to stop receiving commands
  left_socketcan_.wheel_motor_shutdown();
  left_socketcan_.leg_motor_shutdown();
  right_socketcan_.wheel_motor_shutdown();
  right_socketcan_.leg_motor_shutdown();
  return CallbackReturn::SUCCESS;
}

hardware_interface::return_type WheelLeggedRobotHardwareInterface::read()
{
  // TODO(anyone): read robot states

  /* 读取右腿关节位置 */
  // 右腿关节电机原始位置
  double right_leg_motor_position_raw = right_socketcan_.leg_motor_position_read();
  // 将原始位置映射到控制器需要的 0 -> 34度 所对应的弧度, 模型关节z轴朝右侧
  hw_position_states_[right_thigh_link_joint_] = right_leg_motor_position_raw - (M_PI - 34.0*M_PI/180.0);

  /* 读取左腿关节位置 */
  // 左腿关节电机原始位置
  double left_leg_motor_position_raw = left_socketcan_.leg_motor_position_read();
  // 将原始位置映射到控制器需要的 0 -> 34度 所对应的弧度, 模型关节z轴朝右侧
  hw_position_states_[left_thigh_link_joint_] = - (left_leg_motor_position_raw - (M_PI + 34.0*M_PI/180.0));

  /* 读取右轮毂电机位置 */
  // 右轮毂电机原始位置
  double right_wheel_motor_position_raw = right_socketcan_.wheel_motor_angle_read();
  // 将原始位置映射到控制器需要的弧度, 模型关节z轴朝右侧
  hw_position_states_[right_wheel_link_joint_] = - (right_wheel_motor_position_raw - right_wheel_position_offset_);

  /* 读取左轮毂电机位置 */
  // 左轮毂电机原始位置
  double left_wheel_motor_position_raw = left_socketcan_.wheel_motor_angle_read();
  // 将原始位置映射到控制器需要的弧度, 模型关节z轴朝右侧
  hw_position_states_[left_wheel_link_joint_] = left_wheel_motor_position_raw - left_wheel_position_offset_;

  /* 读取右轮毂电机速度 */
  hw_velocity_states_[right_wheel_link_joint_] = - right_socketcan_.wheel_motor_velocity_read();

  /* 读取左轮毂电机速度 */
  hw_velocity_states_[left_wheel_link_joint_] = left_socketcan_.wheel_motor_velocity_read();

  // 通过大腿关节速度接口传递对应轮毂电机转矩电流
  // hw_velocity_states_[right_thigh_link_joint_] = right_socketcan_.wheel_motor_iq_read();
  // hw_velocity_states_[left_thigh_link_joint_] = left_socketcan_.wheel_motor_iq_read();

  // 通过大腿关节速度接口传递对应大腿电机扭矩
  // hw_velocity_states_[right_thigh_link_joint_] = right_socketcan_.leg_motor_effort_read();
  // hw_velocity_states_[left_thigh_link_joint_] = left_socketcan_.leg_motor_effort_read();

  /* 读取右腿关节扭矩 */
  hw_effort_states_[right_thigh_link_joint_] = 0.0; // right_socketcan_.leg_motor_effort_read();
  /* 读取左腿关节扭矩 */
  hw_effort_states_[left_thigh_link_joint_] = 0.0; // - left_socketcan_.leg_motor_effort_read(); // 大腿关节电机, 面向输出端, 逆时针旋转为正

  //RCLCPP_INFO(rclcpp::get_logger("wheel_legged_robot"), "right_thigh_position: %f, left_thigh_position: %f, right_wheel_position: %f\n", hw_position_states_[right_thigh_link_joint_], hw_position_states_[left_thigh_link_joint_], hw_position_states_[right_wheel_link_joint_]);
  //RCLCPP_INFO(rclcpp::get_logger("wheel_legged_robot"), "right_motor_iq: %f\n", hw_velocity_states_[right_thigh_link_joint_]);
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type WheelLeggedRobotHardwareInterface::write()
{
  // TODO(anyone): write robot's commands'
  // 所有关节模型建系中z轴均朝右侧
  // 写入右腿关节控制力矩
  right_socketcan_.leg_motor_effort_control(hw_effort_commands_[right_thigh_link_joint_]);
  // 写入左腿关节控制力矩
  left_socketcan_.leg_motor_effort_control(-hw_effort_commands_[left_thigh_link_joint_]);
  // 写入右轮毂电机控制力矩
  right_socketcan_.wheel_motor_effort_control(-hw_effort_commands_[right_wheel_link_joint_]);
  // 写入左轮毂电机控制力矩
  left_socketcan_.wheel_motor_effort_control(hw_effort_commands_[left_wheel_link_joint_]);

  return hardware_interface::return_type::OK;
}

}  // namespace my_hardware_interface

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  my_hardware_interface::WheelLeggedRobotHardwareInterface, hardware_interface::SystemInterface)
